﻿#include "HRIR.h"
#include "lang/StringFormat.hpp"
#include "io/DataInputStream.h"
#include "util/Util.h"
#include "dsp/BufferDelayer.h"
using namespace yzrilyzr_lang;
using namespace yzrilyzr_array;
using namespace yzrilyzr_io;
using namespace yzrilyzr_util;
namespace yzrilyzr_dsp{
	HRIR::HRIR(std::shared_ptr<yzrilyzr_array::SampleArray> hrir, int32_t azLength, int32_t elLength, int32_t channels, int32_t cvLength, u_sample_rate hrirSampleRate) :
		lFilter(cvLength),
		rFilter(cvLength),
		hrirSrc(hrir),
		azLength(azLength),
		elLength(elLength),
		cvLength(cvLength),
		channels(channels),
		hrirSampleRate(hrirSampleRate){}
	HRIR::~HRIR(){
		//delete hrir_l;
		//delete hrir_r;
	}
	u_sample HRIR::procDsp(u_sample v){
		if(channel == 0){
			return lFilter.procDsp(v);
		} else if(channel == 1){
			return rFilter.procDsp(v);
		}
		return 0;
	}
	void HRIR::init(u_sample_rate sampleRate){
		lFilter.init(sampleRate);
		rFilter.init(sampleRate);
		if(sampleRate == hrirSampleRate){
			hrir=hrirSrc;
		} else{
			u_sample scale=(u_sample)sampleRate / hrirSampleRate;
			int32_t srcLen=hrirSrc->length;
			int32_t srcLen_1=srcLen - 1;
			int32_t dstLen=srcLen * scale;
			hrir=std::make_shared<SampleArray>(dstLen);
			u_sample * src=hrirSrc->_array;
			u_sample * dst=hrir->_array;
			for(u_index i=0;i < dstLen;i++){
				u_sample scaled=i / scale;
				int32_t sci=(int32_t)scaled;
				//u_sample y0=src[Util::clamp(sci - 1, 0, srcLen_1)];
				u_sample y1=src[Util::clamp(sci, 0, srcLen_1)];
				u_sample y2=src[Util::clamp(sci + 1, 0, srcLen_1)];
				//u_sample y3=src[Util::clamp(sci + 2, 0, srcLen_1)];
				//dst[i]=InterpolateFunction::cubicSpline(scaled, sci, y0, y1, y2, y3);
				dst[i]=InterpolateFunction::linear(scaled, sci, y1, y2);
			}
		}
	}
	void HRIR::resetMemory(){
		lFilter.resetMemory();
		rFilter.resetMemory();
	}
	std::shared_ptr<DSP> HRIR::cloneDSP(){
		return std::make_shared<HRIR>(this->hrir, azLength, elLength, channels, cvLength, hrirSampleRate);
	}
	void HRIR::posChange(){
		u_sample elevation_deg=std::atan2(y, z) * Math::RADIANS_TO_DEGREES;
		u_sample azimuth_deg=std::atan2(x, y * y + z * z) * Math::RADIANS_TO_DEGREES;
		azimuth_deg=pvaldeg(azimuth_deg);
		elevation_deg=pvaldeg(elevation_deg);
		//std::cout << "X: " << std::to_string(x) << " Y: " << std::to_string(y) << " Z: " << std::to_string(z) << std::endl;
		//std::cout << "Yaw: " << std::to_string(yaw) << " Pitch: " << std::to_string(pitch) << std::endl;
		//std::cout << "Ele: " << std::to_string(elevation_deg) << " Azi: " << std::to_string(azimuth_deg) << std::endl;
		u_sample * src=hrir->_array;
		int azi=azimuthIndex(azimuth_deg);
		int eli=elevationIndex(elevation_deg);
		int baseIndex=getHRIRIndex(azi, eli);
		memcpy(lFilter.coeff._array, src + baseIndex, cvLength * sizeof(u_sample));
		memcpy(rFilter.coeff._array, src + baseIndex + cvLength, cvLength * sizeof(u_sample));
	}
	int HRIR::azimuthIndex(u_sample azimuth){
		//index-角度
		static std::vector<u_sample> azimuth_value={-80.0, -65.0, -55.0, -45.0, -40.0, -35.0, -30.0, -25.0, -20.0, -15.0, -10.0, -5.0, 0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0, 45.0, 55.0, 65.0, 80.0};
		int nearest_index=0;
		u_sample min_diff=std::abs(azimuth_value[0] - azimuth);
		for(u_index i=1; i < azimuth_value.size(); ++i){
			u_sample diff=std::abs(azimuth_value[i] - azimuth);
			if(diff < min_diff){
				min_diff=diff;
				nearest_index=i;
			}
		}
		return nearest_index;
	}
	int HRIR::elevationIndex(u_sample elevation){
		return Util::clamp(static_cast<int>(std::round((elevation + 45.0) / 5.625)), 0, elLength - 1);
	}
	u_index HRIR::getHRIRIndex(int32_t az, int32_t el){
		return az * (elLength * channels * cvLength) + el * (channels * cvLength);
	}
	u_sample HRIR::pvaldeg(u_sample angle){
		u_sample angle1=std::atan2(std::sin(angle * Math::DEGREES_TO_RADIANS), std::cos(angle * Math::DEGREES_TO_RADIANS)) * Math::RADIANS_TO_DEGREES;
		if(angle1 < -90.0)angle1+=360.0;
		return angle1;
	}
	void HRIR::cloneParam(DSP * obj1){}
	String HRIR::toString()const{
		return StringFormat::object2string("HRIR");
	}
	std::shared_ptr<HRIR> HRIR::parseHRIR(InputStream & inp){
		DataInputStream din(inp);
		u_sample_rate sampleRate=din.readInt();
		u_index  azLength=din.readInt();
		int32_t elLength=din.readInt();
		int32_t channels=din.readInt();
		int32_t cvLength=din.readInt();
		std::shared_ptr<SampleArray> hrir=std::make_shared<SampleArray>(azLength * elLength * channels * cvLength);
		u_sample * ptr=hrir->_array;
		for(u_index i=0, j=hrir->length;i < j;i++){
			ptr[i]=din.readDouble();
		}
		return std::make_shared<HRIR>(hrir, azLength, elLength, channels, cvLength, sampleRate);
	}
}